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(54) FORCE CONTROL ROBOT SYSTEM WITH VISUAL SENSOR FOR INSERTING WORK 



(57) A force-controlled robot system with a visual 
sensor capable of performing a fitting operation auto- 
matically with high reliability. A force sensor (3) attached 
to a wrist portion (2) of a robot (1) detects force in six 
axis directions for force control, and transmits the 
results of detection to a robot controller (5). Position and 
orientation of a convex portion (71) of a fit-in workpiece 
(7) held by claws (41) of a robot hand (4) and position 
and orientation of a concave position (81) of a receiving 
workpiece (8) positioned by a positioning device (9) are 
detected by a three-dimensional visual sensor including 
a structured light unit SU and an image processor in the 
robot controller (5), and a robot position to start an 
inserting action is corrected. Then, the convex portion 
(71) is inserted into the concave portion (81) under the 
force control. After the inserting action completes, it is 
determined whether or not the insertion state of the fit- 
in workpiece (7) in the receiving workpiece (8) is nor- 
mal. 
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Description 

Technical Field 

The present invention relates to a technology of 5 
automating a fitting operation required in an assembling 
process of parts and the like, and more specifically to a 
force-controlled roboi system with a visual sensor in 
which function of the visual sensor is utilized before and 
after an inserting action of the force-controlled robot. io 

Background Art 

Fitting operation is one of basic operations involved 
in most of assembling processes, and automation of the 15 
fitting operation using a robot is already carried out. In 
the fitting operation with a robot, one workpiece (fit-in 
workpiece) is held by the robot and inserted into a pre- 
determined portion (normally a concave portion) of the 
other workpiece (receiving workpiece). A force-control- 20 
led robot is often adopted as the robot for holding the fit- 
in workpiece. 

A force-controlled robot is preferably adopted for 
the fitting operation since it has a function of absorbing 
fluctuation in relative position and orientation between a 25 
fit-in workpiece and a receiving workpiece, and control- 
ling force or moment acting between the workpieces on 
predetermined conditions. However, even when the 
force-controlled robot is used for automation of the fit- 
ting operation, the following items (1) and (2) have to be 30 
settled for performing the fitting operation with high reli- 
ability. 



high degree. Further, when the holding state of the 
workpiece in step (i) (the position and orientation of the 
workpiece relative to the robot hand which is fixed to the 
force sensor) is not normal, the performance of the 
force control based on an output of the force sensor 
drops so that the inserting action is not performed 
smoothly. 

Conventionally, the following measures are taken 
against the above problems: 

a) Using a jig for positioning a receiving workpiece 
with high precision. 

b) Using a hand (end effector) which can hold a fit- 
in workpiece regularly with high reproducibility. 

c) Correcting the position and orientation of a robot 
using a visual sensor. 

Even when the above measure a) is adopted, a 
workpiece may not be positioned with high precision if 
the workpiece has a casting surface of low shaping pre- 
cision. Further, even when the above measure b) is 
adopted, a fit-in workpiece is apt to be held with an inex- 
act orientation if the shaping precision of the workpiece 
is low. The above measure c) of correcting the position 
and orientation of a robot using a visual sensor is known 
as a general technique. However, such technique is not 
known that the relative position/orientation between an 
object held by a robot and an object placed apart from 
the robot is detected and adjustment of the relative posi- 
tion/orientation of the objects is assured prior to a force- 
controlled fitting action which requires the positioning 
with high precision. 



(1) Determination of relative position and orientation 
between a fit-in workpiece and a receiving workpiece 
(e.g., assembling parts) 

Generally, fitting operation with a force-controlled 
robot is performed through the steps of (i) holding a fit- 
in workpiece, which has been positioned at a predeter- 
mined holding position, by a robot hand fixed to a force 
sensor, (ii) moving the robot to a predetermined 
approach position (an inserting action start position) 
and positioning it at the position, (iii) making the robot 
perform a force-controlled inserting action, and (iv) 
releasing the holding of the fit-in workpiece by the robot 
hand and making the robot retreat. 

The relative position and orientation between the 
fit-in workpiece and the receiving workpiece after the 
completion of approaching action in step (ii) is impor- 
tant. At that time, if the relative position and orientation 
between the workpieces is not suitable for starting the 
inserting action, it is difficult to smoothly perform the 
inserting action in step (iii). In particular, when an insert 
portion (convex portion) of the fit-in workpiece and a 55 
receiving portion (concave portion) of the receiving 
workpiece are in tight relation in size and shape, the rel- 
ative position and orientation need to be adjusted at a 



(2) Confirmation of the fitting operation after it has done 

Even if precision of the operation is made higher by 
taking the measures a) to c). it is virtually impossible to 
prevent an occurrence of an abnormal inserting action 
perfectly. Therefore, in order to increase the reliability of 
the system, it is necessary to discriminate whether or 
not an inserting action is normally done. 

Conventionally, the discrimination on the normal- 
ity/abnormality of the inserting action is made based on 
information such as a force sensor output, a torque out- 
put of each axis, a moving amount of a robot hand tip 
point indirectly obtained by outputs from pulse coders of 
respective axes during the inserting action. For exam- 
ple, if a force sensor output or a torque output of each 
axis shows an abnormal value during an inserting 
action, it is discriminated that a fit-in workpiece receives 
a large reactive force exceeding a normal value from a 
receiving workpiece, and if a moving amount of a robot 
hand tip point after a start of insertion is smaller than a 
predetermined insertion length, it is discriminated that 
an obstruction has occurred during the inserting action. 

The discriminating method using a force sensor 
output and a torque output of each axis during an insert- 
ing action is merely an indirect method, so that it is not 
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completely certain. For example, when the holding state 
of a fit-in workpiece is affected by a reactive force from 
a receiving workpiece and deviates from a normal state 
so that an insertion is performed imperfectly, it may be 
discriminated "normal". s 

In the method of detecting a moving amount of a 
hand tip point from outputs of the pulse coders of 
respective axes, as detection error due to elastic defor- 
mations of respective mechanical parts of a robot by a 
reactive force from a receiving workpiece is not negligi- 10 
ble, it is difficult to correctly discriminate whether or not 
an insertion of an intended insertion length has been 
actually done. 

Disclosure of Invention 15 

An object of the present invention is to provide a 
force-controlled robot system capable of adjusting rela- 
tive positional and orientation between a fit-in workpiece 
and a receiving work piece prior to a force-controlled 20 
inserting action in order to smoothly perform the insert- 
ing action. Another object of the present invention is to 
provide a force-controlled robot system capable of 
immediately discriminating whether or not the inserting 
action has carried out normally utilizing a function of the 25 
visual sensor after the inserting action. 

A force-controlled robot system according to the 
present invention has a robot having a robot hand for 
holding a first workpiece and a force sensor for detect- 
ing force applied to the first workpiece held by the robot 30 
hand, a visual sensor for obtaining image data for 
obtaining relative position/orientation between the first 
workpiece and a second workpiece, and a control 
means for controlling the robot and the visual sensor. 

The control means includes a fitting action perform- 35 
ing means for making the first workpiece held by the 
robot hand approach the second workpiece and per- 
forming a fitting action under force control based on an 
output from the force sensor, and a correcting means for 
obtaining workpiece position/orientation data represent- 40 
ing the relative position/orientation between the first 
workpiece and the second workpiece based on image 
data obtained by the visual sensor, and for correcting 
position and orientation of the robot based on the 
obtained workpiece position/orientation data, in 4s 
advance of the fitting action. 

The control means further includes a discriminating 
means for obtaining fitting state data representing fitting 
state of the first workpiece in the second workpiece 
based on image data of the first and second workpieces so 
obtained by the visual sensor and discriminating 
whether or not the fitting state is normal based on the 
obtained fitting state data after completing the fitting 
action. 

The correcting means may obtain the workpiece 55 
position/orientation data based on both of image data of 
the first workpiece and image data of the second work- 
piece obtained by the visual sensor. 



In a typical embodiment, the correcting means cor- 
rects position and orientation of the robot based on 
comparison between the obtained workpiece posi- 
tion/orientation data and workpiece position/orientation 
data taught in advance to the control means. In a pre- 
ferred embodiment, the correcting means obtains the 
workpiece position/orientation data based on at least 
one of image data of the first workpiece and image data 
of the second workpiece obtained at a holding state 
detecting position which is set in the vicinity of a position 
where the fitting action is started, and corrects position 
and orientation of the robot at the fitting action starting 
position. 

Brief Description of Drawings 

FIG. 1 is a perspective view schematically showing 
an entire arrangement for performing a fitting oper- 
ation using a force-controlled robot system accord- 
ing to an embodiment of the present invention; 
FIG. 2a is a schematic diagram showing principal 
part of a structured light unit SU; FIG 2b is a sche- 
matic diagram showing how a structured light is 
formed; 

FIG. 3 is a block diagram showing structure of a 
robot controller as a control means for controlling 
the entire robot system and connecting state with 
other system components; 

FIG. 4 is a schematic diagram for showing the 
essential points of teaching to the robot controller; 
FIG. 5 is a flowchart showing a sequence for one 
cycle of fitting operation which is carried out by a 
playback operation; 

FIG. 6 is a schematic diagram for showing detection 
of position and orientation of a convex portion of a 
fit-in workpiece and detection of position and orien- 
tation of a concave portion of a receiving work- 
piece; and 

FIG. 7 is a schematic diagram for showing detection 
for discrimination on whether or not an inserting 
action is normal. 

Best Mode of Carrying out the Invention 

FIG. 1 schematically shows an entire arrangement 
for performing a fitting operation using a force-controlled 
robot system according to an embodiment of the 
present invention. In FIG. 1, a robot 1 is connected by a 
cable CB1 to a robot controller 5, and has a force sensor 
3 attached to a wrist portion 2 thereof. The force sensor 
3 comprises a bridge circuit including a strain gage etc.. 
and detects force acting on a detecting portion of the 
force sensor 3 in 6 axis directions and transmit the 
result of detection through a cable CB2 to the robot con- 
troller 5 for the force control of the robot 1. 

A hand 4 mounted/fixed to the force sensor 3 per- 
forms opening and closing of holding claws 41 at an 
appropriate robot position to hold a fit-in workpiece 7. 
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The fit-in workpiece 7 in this embodiment is a stepped 
cylindrical assembly component having a convex por- 
tion 71 and a bottom face 72. On the other hand, a 
receiving workpiece 8 is an assembly component with a 
cylindrical portion 82 having a concave portion 81 . The 
receiving workpiece 8 is supplied and positioned on a 
positioning device 9 placed on a work table TB. A sym- 
bdl ZU represents a work coordinate system fixed to the 
positioning device 9, which is set to the robot controller 
5 in advance. 

The positioning device 9 clamps the workpiece 8 in 
two directions (± X-axis directions or ± Z-axis directions) 
or in four directions (± X-axis directions and ± Z-axis 
directions), e.g. by a driving force of air cylinders (not 
shown), to fix the workpiece 8 on a plane parallel to the 
table TB. The drive control of the air cylinders may be 
performed by the robot controller 5. 

The concave portion 81 of the receiving workpiece 
8 is formed to be concentric with the cylindrical portion 
82, and has such dimensions as to tightly receive the 
convex portion 71 of the fit-in workpiece 7. 

A structured light unit SU is connected by a cable 
CB3 to the robot controller 5, and constitutes a three- 
dimensional visual sensor together with the robot con- 
troller 5 having an image processing function in the sys- 
tem. The structured light unit SU is disposed on the 
table TB at a position such that a structured light (slit 
light) is appropriately projected onto the concave por- 
tion 81 of the receiving workpiece 8 and an end of the 
convex portion 71 of the fit-in workpiece 7 which has 
approached the concave portion 81 . As described later, 
the structured light unit SU is used for correcting the 
position and orientation of the robot 1 before the insert- 
ing action thereof, and also used as an inserting action 
confirming means after the inserting action. 

FIG. 2a schematically shows a principal structure of 
the structured light unit SU, and FIG. 2b shows how a 
structured light is formed. The structured light unit SU 
shown in FIG. 2a is designed to project a slit light as a 
structured light SL. A projecting section of the unit 
includes a laser oscillator 12, a cylindrical lens 13. a gal- 
vanometer 14 having a deflecting mirror (driven by a 
stepping motor) and a projection window 11, and an 
image pick-up section includes a CCD camera 20 and 
an image pick-up window 21 . 

As shown in FIG. 2b, when the structured light unit 
SU receives a detection command from the robot con- 
troller 5, a laser beam is emitted from the laser oscillator 
12 and converted by the cylindrical lens 13 to a slit light 
SL. The slit light SL is deflected by the galvanometer 14 
which can be driven at a high speed by a stepping motor 
in a direction in accordance with a command value indi- 
cating a projecting direction, and projected through the 
projection window 11 onto a measuring object 16. An 
image including a bright line 15 formed on the measur- 
ing object 16 is picked up by the CCD camera 20 and 
taken in the robot controller 5 including an image proc- 
essor. 



The robot controller 5 analyzes the image including 
a bright line by an image processing function to obtain 
three-dimensional positions of end points 1 51 , 152, etc. 
of the bright line 15. The principle of obtaining the three 

5 dimensional positions of end points 151 , 152 etc., a cal- 
ibration method and a related calculation process are 
well-known and therefore the detailed explanation 
thereof is omitted. 

FIG. 3 shows internal architecture of the robot con- 

io troller 5 which provides means for controlling the entire 
system, and connection with other system components. 
The robot controller 5 is of a type having an image proc- 
essor, and has a central processing unit (CPU) 51. The 
CPU 51 is connected to a memory 52 in the form of 

75 ROM, a memory 53 in the form of RAM, a non-volatile 
memory 54, a teaching operation panel 55 with a liquid 
crystal display, a digital servo circuit 56 for controlling 
each axis of a robot, a structured light unit interface 61, 
an image processor 62, a monitor interface 63, a frame 

20 memory 64, a program memory 65, a data memory 66 
and a general interface 67 through a bus 58. 

The digital servo circuit 56 is connected through a 
servo amplifier 57 to a mechanical part of a robot RB in 
order to control each axis of the robot 1 . The structured 

25 light unit interface 61 is connected to the above 
described structured light unit SU, and the monitor inter- 
face 63 is connected to a monitor display MO in the form 
of CRT, for example. A variety of external devices can 
be connected to the general interface 67, when neces- 

30 sary. In this embodiment, although a force sensor 3 is 
connected, air cylinders of the above described posi- 
tioning device 9 may be connected. 

The ROM 52 stores a system program necessary 
for controlling respective parts of the system. The RAM 

35 53 is used for temporary storage of data and calcula- 
tion. The non-volatile memory 54 stores data of opera- 
tion programs providing operation commands for 
external devices such as the robot RB, the structured 
light unit SU, the force sensor 2, etc., set data of various 

40 coordinate systems (a work coordinate system EU, a 
force sensor coordinate system, etc.), calibration data 
for three-dimensional visual sensor (data used for con- 
verting the output of the three-dimensional visual sen- 
sor expressed in the sensor coordinate system XS into 

45 position and orientation data in a robot coordinate sys- 
tem), related set values, and so forth. 

The structured light unit interface 61 is used when 
commands for controlling respective parts of the struc- 
tured light unit SU are transmitted and when images 

so picked up by the CCD camera 20 (see FIG. 2) are taken 
in. The images taken in the robot controller 5 are con- 
verted to gray scale, and once stored in the frame mem- 
ory 64. The images stored in the frame memory 64 are 
ready to be displayed on the monitor display MO. 

55 The program memory 65 stores programs for 
image processing and analysis using the image proces- 
sor 62, and the data memory 66 stores set data related 
to the image processing and analysis, and so forth. The 
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following data 1) and 2) are stored in the memory 
according to the characteristics of the present invention. 
Processing according to the above mentioned programs 
will be described later. 

5 

1) Program data prescribing processes for obtain- 
ing position and orientation of the convex portion 71 
of the fit-in workpiece 7 and the concave portion 81 
of the receiving workpiece 81 in order to correct 
position and orientation of the robot 1 before start- 10 
ing the inserting action, and the related set data. 

2) Program data prescribing processes for discrim- 
inating whether or not the inserting action has been 
performed normally after the robot 1 completes the 
inserting action, and the related set data. 15 

Based on the above premise, the process of per- 
forming fitting operation in the present embodiment will 
be described together with the related precessing. 

20 

[1] Teaching of a fitting operation to the robot 

A fitting operation is taught in order to make the 
robot 1 perform fitting operation on a plurality of paired 
workpieces 7 and 8 by repeating a playback operation 25 
according to an operation program. Summary of teach- 
ing will be described below referring to FIG. 4. A tool 
coordinate system ST is set such that an origin thereof 
coincides with a center point (TCP) of a tip face 73 of a 
fit-in workpiece 7 which is held by a hand 4 according to 30 
the teaching, and the direction of a cylinder axis of the 
fit-in workpiece 7 coincides with the direction of Z-axis. 

1) A holding position Ph (including an orientation, 
which will apply hereinafter) is taught for holding by 35 
a hand 4 a fit-in workpiece 7 (a master workpiece 

for teaching) supplied at a predetermined supply 
position with a predetermined orientation. A matrix 
representing the taught holding position in a base 
coordinate system SB is denoted by H. *o 

2) A holding action by the hand 4 (an action of 
opening claws 41) is taught. 

3) A holding state detecting position Pd selected 
within a range 17 which is suitable for position 
measurement (slit light projection) by a structured 45 
light unit SU is taught. A matrix representing the 
taught holding state detecting position Pd in the 
base coordinate system SB is denoted by D. It is to 

be noted that the structured light unit SU is dis- 
posed at such position that is also suitable for dis- so 
criminating whether or not the inserting action is 
normally done (which will be described later). 
Therefore, the holding state detecting position Pd is 
selected so that it is not far from a concave portion 
81 of a receiving workpiece 8. and so that the struc- 55 
tured light unit SU can detect image data on a con- 
vex portion 71 of the fit-in workpiece 7. 

4) Conditions of movement from the holding posi- 



tion Ph to the holding state detecting position Pd 
are designated. In the present example, "movement 
by respective axes" and "positioning ratio 100% 
(stopping at the position Pd)" are designated. Also 
an appropriate speed is designated. 

5) An approach position Pa suitable for starting an 
action of inserting the convex portion 71 into the 
concave portion 81 is taught. A matrix representing 
the taught approach position is denoted by A. The 
approach position Pa is taught as position and ori- 
entation of the robot in which the convex portion 71 
faces and in alignment with the concave portion 81 
on condition that the positioning of the receiving 
workpiece 8 and the holding of the fit-in workpiece 
7 are in an ideal state. 

6) Conditions of movement from the holding state 
detecting position Pd to the approach position Pa 
are designated. In the present example, "straight- 
line movement" and "positioning ratio 100% (com- 
plete stopping at the position Pa)" are designated. It 
is to be noted that the moving action is corrected by 
an approach action correction, as described later. 

7) Conditions of force-controlled inserting action 
are designated. The conditions to be taught are as 
follows: 

(i) Force control is made effective only on a 
force Fz of Z-axis direction in the tool coordi- 
nate system ST. The magnitude of a target 
force is designated appropriately by tuning. 
Force control is ineffective in the X-axis direc- 
tion, in the Y-axis direction, around the X-axis, 
around the Y-axis and around the Z-axis. A 
motion command containing these compo- 
nents is not outputted to the servo system 
(these components are maintained as they are 
at the approach position). 

(ii) An insertion length L is designated. The 
insertion length L is designated through man- 
ual input according to design data. 

(iii) An index is set for discriminating whether or 
not an inserting action is completed. In this 
example, a reference value to is set for time 
passed after a start of an inserting action. The 
time passed after a start of an inserting action 
is measured and used as an index for discrimi- 
nating whether or not the inserting action is 
completed together with the insertion length L. 
Specifically, when at least one of the movement 
of distance L and the elapse of time to meas- 
ured from the start of an approach action is 
detected, the robot is stopped and the force 
control is ceased. 

8) An action of ceasing the holding by hand 4 (an 
action of opening the claws 41) and a retreat action 
of the robot. As a robot retreat point, a position suit- 
able for the next operation cycle is taught (not 
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shown in the drawings). 

[2] Teaching of detecting actions by the three-dimen- 
sional visual sensor 

Commands for performing detecting actions by the 
three-dimensional visual sensor are written in the oper- 
ation program. The taught detecting actions and timing 
for the actions are as follows: 



1) Detection of the position and orientation of the 
convex portion 71 of the workpiece 7 held by the 
robot 1. The detection is performed between the 
time when the robot 1 is positioned at the holding 
state detecting position Pd and the time when the is 
approach action is started. In the present example, 
the detection is performed immediately after the 

. robot 1 is positioned at the holding state detecting 
position Pd. 

2) Detection of the position and orientation of the 20 
concave portion 81 of the receiving workpiece 8. 
The detection is performed at any time after the 
positioning of the receiving workpiece 8 by the posi- 
tioning device 9 is completed and before the 
approach action of the robot 1 is started. In the 25 
present example, the detection is performed imme- 
diately after the detection of position and orienta- 
tion of the convex portion 71 as described at the 
item 1) is completed. 

3) Detection for determining whether or not the 30 
inserting action by the robot 1 is normal. In this 
example, the position and orientation of a rear face 
72 of the fit-in workpiece 7 is detected, as 
described later. The detection is performed immedi- 
ately after the robot 1 ceases the holding of the 35 
workpiece 7 and retreats to the retreat position. 

FIG. 5 is a flowchart showing a sequence of one 
cycle of the fitting operation which is performed by a 
playback operation after the teaching is completed in 40 
the above described manner. Each Step S1-S15 is brei- 
f ly described as follows: 



Step S1 : Moving the robot 1 from a waiting position 
to the taught holding position Ph and positioning the 
robot there. 

Step S2: Holding the fit-in workpiece 7 by an open- 
ing action of the claws 41 of the hand 4. 
Step S3: Moving the robot 1 to the taught holding 
state detecting position Pd and positioning the 
robot 1 there. 

Step S4: Detecting the position and orientation of 
the convex portion 71 of the fit-in workpiece 7 held 
by the robot 1. The way of detection will be 
described later. 

Step S5: Detecting the position and orientation of 
the concave portion 81 of the receiving workpiece 
8. 



Step S6: Moving the robot 1 to a corrected 
approach position Pa' which is determined by cor- 
recting the taught approach position Pa based on 
the results of detection at Steps S4 and S5. 
s Step S7: Starting a force-controlled inserting 

action. Since the details of the force-controlled 
inserting action are generally known, the explana- 
tion thereof is omitted. In this example, an imped- 
ance control of the robot is performed in 
10 accordiance with the above described manner. 
Thus, the robot 1 moves in the Y-axis direction and 
at the same time comes to the state of outputting 
the designated target force Fz in the Y-axis direc- 
tion. 

Step S8: Waiting for detection of the movement of 
distance L or the elapse of time to measured from 
the start of an approach action. 
Step S9: Stopping the robot and ceasing the force 
control. 

Step S10: Releasing the fit-in workpiece 7 from the 
holding by an opening action of the claws 41 of the 
hand 4. 

Step S1 1 : Retreat the robot 1 to the taught retreat 
position. 

Step S12: Detecting the position and orientation of 
the bottom face 72 of the fit-in workpiece 7 held by 
the robot 1 . 

Step S13: Discriminating whether or not the inser- 
tion state of the convex portion 71 in the concave 
portion 81 is normal. 

Step S14: Outputting a signal indicative of a normal 
insertion state if the insertion state is normal. 
Based thereon, an indication such as "normal inser- 
tion" is displayed on the display screen of the teach- 
ing operation panel 55. 

Step S15: Outputting an alarm signal indicative of 
an abnormal insertion state if the insertion state is 
not normal. Based thereon, an indication such as 
"abnormal insertion" is displayed on the display 
screen of the teaching operation panel 55 and also 
an alarm sound is made, to brought the system to 
an emergency stop. 



Trie following is a supplementary description on the 
45 detection by the three-dimensional visual sensor, and 
the correction of the robot action and the discrimination 
on the insertion state based on the detection, which are 
contained in the above sequence. 

50 [I] Detection of position and orientation of the convex 
portion 71 of the fit-in workpiece 7 (Step S4) 

As shown in FIG. 6, a slit light is projected some 
times (in this example, two times) from the projecting 
55 section of the structured light unit SU to form bright lines 
ab and cd on the end face 73 of the fit-in workpiece 7, 
successively. The images of the bright lines ab and cd 
are picked up by the camera of the structured light unit 
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SU. The obtained images are analyzed in the robot con- 
troller 5 to obtain the three-dimensional positions of the 
end points a, b, c and d of the bright lines ab and cd. 
From the positions of those four points, a center position 
and an orientation of the circular face 73 is obtained by 
least square approximation, for example. A coordinate 
system fixed to the center of the circular face 73 is 
denoted by XC. A matrix (data in the sensor coordinate 
system XS) representing the position and orientation of 
the coordinate system XC obtained from sensor output 
during a playback operation (actual operation) is 
denoted by C\ 

The similar detection is performed in teaching the 
detecting action by the three-dimensional visual sensor, 
as described before. Referring to the above described 
definition of the tool coordinate system XT, the position 
and orientation of the convex portion 71 at the time of 
teaching the holding state detection position is equiva- 
lent to the position and orientation of the tool coordinate 
system XT. A matrix (sensor output) representing that 
position and orientation of the tool coordinate system 
XT in the sensor coordinate system XS is denoted by C. 

[II] Detection of the position and orientation of the con- 
cave portion 81 of the receiving workpiece 8 (Step S5) 

As in the detection of the convex portion 71 . a slit 
light is projected some times (in this example, two 
times) from the projecting section of the structured light 
unit SU as shown in FIG. 6 to form bright lines ef, gh and 
bright lines ij, kl on the edge face 83 of the cylindrical 
portion 82, successively. The images of those bright 
lines are picked up by the camera of the structured light 
unit SU. The obtained images are analyzed in the robot 
controller 5 so that the three-dimensional positions of 
the inside end points f, g, j and k of the respective bright 
lines are obtained. From the positions of those four 
points, a center position and an orientation of the circu- 
lar opening of the concave portion 81 is obtained by 
least square approximation, for example. A coordinate 
system fixed to the center of the circular opening is 
denoted by XV, and a matrix (data in the sensor coordi- 
nate system XS) representing the position and orienta- 
tion of the coordinate system XV obtained from sensor 
output during a playback operation (actual operation) is 
denoted by V. 

The similar detection is performed in teaching the 
detecting action by the three-dimensional visual sensor, 
as described before. A matrix (sensor output) repre- 
senting the position and orientation of the coordinate 
system XV, which expresses the position and orientation 
of the concave portion 81 , at the time of teaching in the 
sensor coordinate system XS is denoted by V. 

[Ill] How to obtain the corrected approach position A' 
(Step S6) 

First, correction of deviation in position and orienta- 



tion of the concave portion 81 will be explained. 

In addition to the above described homogeneous 
transformation matrices, homogeneous transformation 
matrices defined as follows are used. Data correspond- 
5 ing to those definitions are stored as designated data on 
each coordinate system in the non-volatile memory 54 
in the robot controller 5. 
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R: A matrix representing the position and orienta- 
tion of a robot face plate coordinate system XF in 
the base coordinate system XB. 
T: A matrix representing the position and orienta- 
tion of the tool coordinate system XT in the face 
plate coordinate system XF. 

U: A matrix representing the position and orienta- 
tion of the work coordinate system XU in the base 
coordinate system XB. 

P: A matrix representing the position and orienta- 
tion of the taught point (holding orientation detect- 
ing position Pd) in the work coordinate system XU. 

At the time the robot reaches the taught holding 
state detecting position Pd, the following expression (1) 
holds true: 



40 



R-T=U-P 



0) 



Therefore, the position of the robot face plate at that 
time is given according to the following expression (2): 



R = U-P-T" 



(2) 



Here, if the position of the face plate corresponding 
to the corrected position of the TCP which compensates 
35 deviation in position and orientation of the concave por- 
tion 81 is denoted by R\ the following expression (3) 
holds true: 



R' • T = U • AU • P 



(3) 



Since the left side represents the position of the 
TCP after the correction for compensating the deviation 
in position and orientation of the concave portion 81, it 
is necessary for carrying out the correction to obtain AU 
45 in the expression (3). 

Now, providing that a matrix representing the coor- 
dinate system XV (the center position and orientation of 
the opening of the concave portion) in the work coordi- 
nate system XU is denoted by Q (unknown fixed matrix) 
so and a matrix representing the position and orientation of 
the sensor coordinate system XS in the base coordinate 
system XB is denoted by S, the following expressions 
(4) and (5) is held at the time of teaching and at the time 
of actual measurement (playback operation), respec- 

55 tively. 

At the time of teaching; 



U-Q=S-V 



(4) 
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At the time of actual measurement; 

U • AU • Q = S • V (5) 
From those expressions, 5 

AU = U~ 1 -S- V'-(U" 1 -S- V)~ 1 (6) 
= U* 1 -S-V'. V" 1 -S" 1 -U 

w 

On the right side of the expression (6). data on U, S 
and V are taught to the robot controller, and V is 
obtained at Step S5. Therefore, AU can be obtained in 
the robot controller. 

Next, correction of deviation in position and orienta- is 
tion of the convex portion 71 will be explained. 

As described above, at the time the robot reaches 
the taught holding state detection position, the above 
expression (1) is held, and the position of the robot face 
plate at that time is given by the above expression (2). 20 

Here, providing that the position of the face plate 
coordinate with the position of the TCP after the correc- 
tion for compensating the deviation in position and ori- 
entation of the convex portion 71 is denoted by R", the 
following expression (7) is held as in the case of the 25 



concave portion 81 : 

R"*T»AT=U-P (7) 
Therefore, 30 
R"-T=U-P-AT" 1 (8) 



Since the left side of the expression (8) represents the 
position of the TCP after the correction for compensat- 35 
ing the deviation in position and orientation of the con- 
vex portion 71, it is necessary for carrying out the 
correction to obtain AT in the expression (8). 

Now, providing that a matrix representing the coor- 
dinate system ZC (the center position and orientation of 40 
the tip end face of the convex portion) in the tool coordi- 
nate system ZT (unknown fixed matrix) is denoted by M, 
the following expressions (9) and (10) are held at the 
time of teaching and at the time of actual measurement 
(playback operation), respectively. as 

At the time of teaching; 

R • T • M = S • C (9) 
At the time of actual measurement; 50 

R-T»AT«MsS«C (10) 
From those expressions, 

55 

AT = T" 1 • R" 1 • S • C"(T 1 • R" 1 • S • C)" 1 (11) 
= T' 1 *R" 1 .S-C'-C" 1 -S" 1 -R-T 



On the right side of the expression (11), data on T, 
R, S and C are taught to the robot controller, and C* is 
obtained at Step S4. Therefore, AT can be obtained in 
the robot controller. 

3) The way of correcting both of deviation in posi- 
tion and orientation of the concave portion 81 and 
deviation"in position and orientation of the convex 
portion 71 will be explained. 

Deviation in position and orientation of the concave 
portion 81 and deviation in position and orientation of 
the convex portion 71 can be both corrected using the 
AU and AT obtained by the above expressions (6) and 
(11). 

Specifically, providing that the position of the face 
plate coordinate with the position of the TCP after the 
correction for compensating both deviations is denoted 
by RCR, the following expression (12) holds true: 

RCR • T • AT = U • AU • P (12) 

As is dear from the expression (12), by making the 
following substitution with respect to the set data U on 
the work coordinate system ZU and the set data T on 
the tool coordinate system ZT using the results of the 
expressions (6) and (11), 

U-»U-AU 

T -> T • AT, 

the correction for compensating deviation in position 
and orientation of both the convex portion 71 and the 
concave portion 81 can be carried out. This applies also 
to the correction for compensating deviation in position 
and orientation of both the convex portion 71 and the 
concave portion 81 with respect of the approach posi- 
tion Pa. Therefore, if the face plate is shifted to the fol- 
lowing position RA* which is obtained by substituting the 
approach position G in the work coorcfinate system ZU 
for the holding state detecting position P in the work 
coordinate system ZU, the approach position Pa is cor- 
rected as Pa'. 

RA'=U-AU-G-(T-A7)" 1 (13) 

In the above equation, the matrix G can be calcu- 
lated according to the following equation using the 
taught approach position Pa and the work coordinate 
system ZU in the base coordinate system ZB. 

G=U' 1 -A (14) 

(d) The way of discriminating whether or not the inser- 
tion state is normal (Steps S12/S13) 

As shown in FIG. 7, a slit light is projected some 
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times (in this example, two times) from the projecting 
section of the structured light unit SU to form bright lines 
mn and op on the tip face 72 of the fit-in workpiece 7, 
successively. The images of those bright lines are 
picked up by the camera of the structured light unit SU, 5 
successively. The obtained images are analyzed in the 
robot controller 5 to obtain the threenjimensional posi- 
tions of the end points m, n, o and p of the respective 
bright lines. 

Then, based on the Z-coordinate values of those 10 
positions (in the base coordinate system ZB), it is dis- 
criminated whether or not the height of the bottom face 
72 measured from the bottom 85 of the concave portion 
81 (in the horizontal direction) is normal (the insertion 
length Lis attained). 75 

For example, an upper limit value Zmax is set for 
the Z-coordinate values of the end points m, n, o and p, 
and if at least one of the end points m, n, o and p 
exceeds the upper limit value Zmax, discrimination is 
made that the insertion is not normal. The abnormal 20 
insertion means, for example, cases such that a gap 84 
larger than a predetermined value remains between the 
tip of the convex portion 71 inserted in the concave por- 
tion 81 and the bottom 85. Discrimination on normality 
of the insertion orientation can be made based on differ- 25 
ences between the Z-coordinate values of the end 
points m, n, 0 and p. 

In the present embodiment, the three-dimensional 
visual sensor is used as a visual sensor. In some cases, 
for example, in the case where a deviation of holding of 30 
the fit-in workpiece 7 is small and one direction compo- 
nent (for example, Y-direction component) of the posi- 
tion of the receiving workpiece 7 is regulated with high 
precision, a two-dimensional visual sensor can be used 
as a visual sensor. Needless to say, also in such cases, 35 
information about correction of the actions of the robot 
and information about whether the insertion state is nor- 
mal or not are obtained. 

According to the present invention, by additionally 
providing one visual sensor to a force-controlled robot 40 
system for performing the fitting operation, for use in 
correcting robot actions and in obtaining information as 
to whether an insertion state is normal or not, the fitting 
operation is automated with high reliability. 

45 

Claims 

1. A force-controlled robot system for performing an 
operation of fitting a first workpiece into a second 
workpiece, comprising: 50 

a robot having a robot hand for holding the first 
workpiece and a force sensor for detecting 
force applied to the first workpiece held by said 
robot hand; 55 
a visual sensor for obtaining image data for 
obtaining relative position/orientation between 
said first workpiece and said second work- 



piece; and 

a control means for controlling said robot and 
said visual sensor, said control means includ- 
ing a fitting action performing means for mak- 
ing said first workpiece held by said robot hand 
approach said second workpiece and perform- 
ing a fitting action under force control based on 
an output from said force sensor, and a correct- 
ing means for obtaining workpiece position/ori- 
entation data representing the relative 
position/orientation between said first work- 
piece and said second workpiece based on 
image data obtained by said visual sensor, and 
for correcting position and orientation of said 
robot based on the obtained workpiece posi- 
tion/orientation data, in advance of said fitting 
action. 

2. A force-controlled robot system according to claim 
1, wherein said correcting means obtains said 
workpiece position/orientation data based on both 
of image data of said first workpiece and image 
data of said second workpiece obtained by said vis- 
ual sensor. 

3. A force-controlled robot system for performing an 
operation of fitting a first workpiece into a second 
workpiece, comprising: 

a robot having a robot hand for holding the first 
workpiece and a force sensor for detecting 
force applied to the first workpiece held by said 
robot hand; 

a visual sensor for obtaining image data for 
obtaining relative position/orientation between 
said first workpiece and said second work- 
piece; and 

a control means for controlling said robot and 
said visual sensor, said control means includ- 
ing a fitting action performing means for mak- 
ing said first workpiece held by said robot hand 
approach said second workpiece and perform- 
ing a fitting action under force control based on 
an output from said force sensor, a correcting 
means for obtaining workpiece position/orien- 
tation data representing the relative posi- 
tion/orientation between said first workpiece 
and said second workpiece based on image 
data obtained by said visual sensor and for cor- 
recting position and orientation of said robot 
based on the obtained workpiece position/ori- 
entation data in advance of said fitting action, 
and a discriminating means for obtaining fitting 
state data representing fitting state of said first 
workpiece in said second workpiece based on 
image data of the first and second workpieces 
obtained by said visual sensor and discriminat- 
ing whether or not the fitting state is normal 
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based on the obtained fitting state data after 
completing said fitting action. 

A force-controlled robot system according to claim 
3, wherein said correcting means obtains said 
workpiece position/orientation data based on both 
of image data of said first workpiece and image 
data of said second workpiece obtained by said vis- 
ual sensor. 
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A force-controlled robot system according to any 
one of claims 1 to 4, wherein said correcting means 
corrects position and orientation of said robot 
based on comparison between said obtained work- 
piece position/orientation data and workpiece posi- 15 
tion/orientation data taught in advance to said 
control means. 

A force-controlled robot system according to any 
one of claims 1 to 4, wherein said correcting means 20 
obtains said workpiece position/orientation data 
based on at least one of image data of said first 
workpiece and image data of said second work- 
piece obtained at a holding state detecting position 
which is set in the vicinity of a position where said 25 
fitting action starts, and corrects position and orien- 
tation of said robot at said fitting action starting 
position. 
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